#include "RGBDFrame.h"
#include "ros/ros.h"
#include "math.h"

RGBDFrame::RGBDFrame()
{
}

RGBDFrame::RGBDFrame(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud)
{

	for(int i = 0; i < input_cloud->height*input_cloud->width; i++)
	{
		if(input_cloud->points[i].normal_x != 0 && !std::isnan(input_cloud->points[i].normal_x) && i%1000 == 0)
		{
			ROS_INFO("Original1:	%f 	%f 	%f",input_cloud->points[i].normal_x,input_cloud->points[i].normal_y,input_cloud->points[i].normal_z);
		}
	}

	width 					= input_cloud->width;
	height 					= input_cloud->height;
	IplImage * normal_img 		= cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 3);
	IplImage * rgb_img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
	int index 					= 0;
	char * rgb_data 			= rgb_img->imageData;
	float * normal_data 		= (float *)(normal_img->imageData);
	for(int j = 0; j < height; j++)
	{
		for(int i = 0; i < width; i++)
		{
			int ind = 3 * (j * width + i);
			rgb_data[ind + 0]=input_cloud->points[index].b; // B
			rgb_data[ind + 1]=input_cloud->points[index].g; // G
			rgb_data[ind + 2]=input_cloud->points[index].r; // R
			if(!std::isnan(input_cloud->points[index].normal_x))
			{
				if(index%1000 == 0){ROS_INFO("Original2:	%f 	%f 	%f",input_cloud->points[index].normal_x,input_cloud->points[index].normal_y,input_cloud->points[index].normal_z);}
				normal_data[ind + 0]=(input_cloud->points[index].normal_x+1.0f)/2.0f; // B
				normal_data[ind + 1]=(input_cloud->points[index].normal_y+1.0f)/2.0f; // G
				normal_data[ind + 2]=(input_cloud->points[index].normal_z+1.0f)/2.0f; // R
			}else{
				normal_data[ind + 0]=0; // B
				normal_data[ind + 1]=0; // G
				normal_data[ind + 2]=0; // R
				input_cloud->points[index].x = 0;
				input_cloud->points[index].y = 0;
				input_cloud->points[index].z = 0;
			}
			/*
			if(input_cloud->points[index].z > 0){
				float nx = input_cloud->points[index].normal_x;
				float ny = input_cloud->points[index].normal_y;
				input_cloud->points[index].normal_z = 1-nx*nx-ny*ny;
				float nz = input_cloud->points[index].normal_z;
				if(std::isnan(nz)){
					input_cloud->points[index].x = 0;
					input_cloud->points[index].y = 0;
					input_cloud->points[index].z = 0;
				}
				normal_data[ind + 0]=nx; // B
				normal_data[ind + 1]=ny; // G
				normal_data[ind + 2]=nz; // R
			}else{
				normal_data[ind + 0]=0; // B
				normal_data[ind + 1]=0; // G
				normal_data[ind + 2]=0; // R
			}*/
			index++;
		}
	}
	

	
	cvNamedWindow("InputCloud_rgb", CV_WINDOW_AUTOSIZE );
	cvShowImage("InputCloud_rgb", rgb_img);
	cvNamedWindow("InputCloud_normal", CV_WINDOW_AUTOSIZE );
	cvShowImage("InputCloud_normal", normal_img);
	
	extractPlanes(input_cloud);
		
    cvWaitKey(0);
	cvReleaseImage( &rgb_img );
	cvReleaseImage( &normal_img );
}

void RGBDFrame::print()
{
	ROS_INFO("id_number: %i",id_number);
	ROS_INFO("rgb_path: %s",rgb_path.c_str());
	ROS_INFO("depth_path: %s",depth_path.c_str());
	ROS_INFO("timing: %f",timing);
}

bool valid_distance(float z){
	if(z > 0 && z < 3500){
		return true;
	}
	else{
		return false;
	}
}

RGBDFrame::RGBDFrame(RGBDCamera * camera, int number)
{
	focal_length_x 		= camera->focal_length_x;
	focal_length_y 		= camera->focal_length_y;
	optical_center_x 	= camera->optical_center_x;
	optical_center_y 	= camera->optical_center_y;
	id_number 			= number;
	ROS_INFO("ID: %i",id_number);
	rgb_path 			= *(camera->rgb_list[id_number]);
	depth_path 			= *(camera->depth_list[id_number]);
	timing 				= camera->timings[id_number];
	keypoints = 0;
	//setupKeyPointData();
	//setupImageDescriptor();
	//showFrame();
	
	IplImage* bw_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_GRAYSCALE);
	IplImage* rgb_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_UNCHANGED);
	IplImage* depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_UNCHANGED );
	width = rgb_img->width;
	height = rgb_img->height;
	
	int nr_points = 0;
	float ** point_x 	= new float*[width];
	float ** point_y 	= new float*[width];
	float ** point_z 	= new float*[width];
	int ** point_r 		= new int*[width];
	int ** point_g 		= new int*[width];
	int ** point_b 		= new int*[width];
	Point *** pointMat 	= new Point**[width]; 
	for(int i = 0; i < width; i++)
	{
		point_x[i] 	= new float[height];
		point_y[i] 	= new float[height];
		point_z[i] 	= new float[height];
		point_r[i] 	= new int[height];
		point_g[i] 	= new int[height];
		point_b[i] 	= new int[height];
		pointMat[i] = new Point*[height]; 
		for(int j = 0; j < height; j++)
		{
			pointMat[i][j] = 0;
			unsigned char d1 		= depth_img->imageData[3 * (j * depth_img->width + i) + 0];
			unsigned char d2 		= depth_img->imageData[3 * (j * depth_img->width + i) + 1];
			point_b[i][j] 			= (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 0]);
			point_g[i][j] 			= (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 1]);
			point_r[i][j] 			= (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 2]);
			
			float ii = i;
			float jj = j;
			
			float z_val 			= (float)(d1*256 + d2);
			float x_val 			= (ii - optical_center_x) * z_val / focal_length_x;
			float y_val 			= (jj - optical_center_y) * z_val / focal_length_x;
			
			point_x[i][j] 			= x_val;
			point_y[i][j] 			= y_val;
			point_z[i][j] 			= z_val;
			if(valid_distance(z_val))	{nr_points++;}
		}
	}
	/*
	struct timeval start, end;
	gettimeofday(&start, NULL);
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	cloud->width 	= width;
	cloud->height 	= height;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width*cloud->height);
	int point_index = 0;
	for(int j = 0; j < height; j++)
	{
		for(int i = 0; i < width; i++)
		{
			float z_val = point_z[i][j];
			if(z_val > 0 && z_val < 3500)
			{
				cloud->points[point_index].x = point_x[i][j];
				cloud->points[point_index].y = point_y[i][j];
				cloud->points[point_index].z = point_z[i][j];
			}else{
				cloud->points[point_index].x = 0;//NAN;
				cloud->points[point_index].y = 0;//NAN;
				cloud->points[point_index].z = 0;//NAN;
			}
			cloud->points[point_index].r = point_r[i][j];
			cloud->points[point_index].g = point_g[i][j];
			cloud->points[point_index].b = point_b[i][j];
			point_index++;
		}
	}
	
	
	gettimeofday(&end, NULL);
	ROS_INFO("\Plane extraction execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	ROS_INFO("DONE\n");
	exit(0);
	*/
	Point ** points = new Point*[nr_points];
	int counter = 0;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			if(valid_distance(point_z[i][j])) 
			{
				points[counter] 	= new Point(point_x[i][j],point_y[i][j],point_z[i][j],i,j);
				pointMat[i][j] 		= points[counter];
				counter++;
			}
		}
	}
	for(int i = 0; i < nr_points; i++)
	{
		Point * tmp_point = points[i];
		int switch_index = rand()%nr_points;
		points[i] = points[switch_index];
		points[switch_index] = tmp_point;
	}
	
	ROS_INFO("%i",__LINE__);

	FeatureExtractor * fe = new SurfExtractor();
	keypoints = fe->getKeyPointSet(bw_img,pointMat,point_r,point_g,point_b);
	
	ROS_INFO("%i",__LINE__);
	
	extractPlanes(pointMat,points,nr_points,point_x,point_y,point_z,rgb_img,depth_img);
	
	ROS_INFO("%i",__LINE__);
	
	delete fe;
	cvReleaseImage( &bw_img );
    cvReleaseImage( &rgb_img );
	cvReleaseImage( &depth_img );
	
	ROS_INFO("%i",__LINE__);
	//exit(0);
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			if(pointMat[i][j] != 0 && pointMat[i][j]->keypoint == 0){delete pointMat[i][j];}
		}
		delete point_x[i];
		delete point_y[i];
		delete point_z[i];
		delete point_r[i];
		delete point_g[i];
		delete point_b[i];
		delete pointMat[i]; 
	}
	delete point_x;
	delete point_y;
	delete point_z;
	delete point_r;
	delete point_g;
	delete point_b;
	delete pointMat;
	delete points; 
	//showFrame();
	//Clear mats
}

RGBDFrame::~RGBDFrame(){
	printf("~RGBDFrame()\n");
	if(keypoints == 0){delete keypoints;}
	//exit(0);
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr RGBDFrame::getCloud()
{
	IplImage* rgb_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_UNCHANGED);
	IplImage* depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_UNCHANGED );
	int nr_particles = 0;
	for(int i = 0; i < width; i++ )
	{
		for(int j = 0; j < height; j++ )
		{
			unsigned char d1 		= depth_img->imageData[3 * (j * depth_img->width + i) + 0];
			unsigned char d2 		= depth_img->imageData[3 * (j * depth_img->width + i) + 1];
			float z_val 			= (float)(d1*256 + d2);
			if(z_val > 0 && z_val < 20000){nr_particles++;}
		}
	}

	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp_cloud(new pcl::PointCloud<pcl::PointXYZRGB>());
	tmp_cloud->width = nr_particles;
	tmp_cloud->height = 1;
	tmp_cloud->is_dense = false;
	tmp_cloud->points.resize(nr_particles);
	
	int nr_points = 0;
	for(int i = 0; i < width; i++)
	{
		for(int j = 0; j < height; j++)
		{
			unsigned char d1 		= depth_img->imageData[3 * (j * depth_img->width + i) + 0];
			unsigned char d2 		= depth_img->imageData[3 * (j * depth_img->width + i) + 1];			
			float ii = i;
			float jj = j;
			
			float z_val 			= (float)(d1*256 + d2);
			float x_val 			= (ii - optical_center_x) * z_val / focal_length_x;
			float y_val 			= (jj - optical_center_y) * z_val / focal_length_y;
			if(z_val > 0 && z_val < 20000)
			{
				tmp_cloud->points[nr_points].x = x_val;
				tmp_cloud->points[nr_points].y = y_val;
				tmp_cloud->points[nr_points].z = z_val;
				tmp_cloud->points[nr_points].r = (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 2]);
				tmp_cloud->points[nr_points].g = (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 1]);
				tmp_cloud->points[nr_points].b = (unsigned char)(rgb_img->imageData[3 * (j * rgb_img->width + i) + 0]);
				nr_points++;
			}
		}
	}
	return tmp_cloud;
}

float max_angle_diff 	= 0.15f;

void RGBDFrame::extractPoints(int &ipa,int &ipb,int &ipc,pcl::PointXYZRGBNormal * & pa, pcl::PointXYZRGBNormal * & pb, pcl::PointXYZRGBNormal * & pc,pcl::PointXYZRGBNormal ** valid_points,int valid_counter){
	ipa = rand()%valid_counter;
	ipb = rand()%valid_counter;
	ipc = rand()%valid_counter;
	pa = valid_points[ipa];
	pb = valid_points[ipb];
	pc = valid_points[ipc];
	
	float papb_angle = pa->normal_x*pb->normal_x + pa->normal_y*pb->normal_y + pa->normal_z*pb->normal_z;
	int counter = 0; 
	while(counter < 25 && (pb == pa || fabs(papb_angle)<(1-max_angle_diff)))
	{
		ipb = rand()%valid_counter;
		pb = valid_points[ipb];
		papb_angle = pa->normal_x*pb->normal_x + pa->normal_y*pb->normal_y + pa->normal_z*pb->normal_z;
	}
	if(counter < 25){
		float papc_angle = pa->normal_x*pc->normal_x + pa->normal_y*pc->normal_y + pa->normal_z*pc->normal_z;
		float pbpc_angle = pb->normal_x*pc->normal_x + pb->normal_y*pc->normal_y + pb->normal_z*pc->normal_z;
		while(counter < 65 && (pc == pa || pc == pb || fabs(papc_angle)<(1-max_angle_diff) || fabs(pbpc_angle)<(1-max_angle_diff)))
		{
			ipc = rand()%valid_counter;
			pc = valid_points[ipc];
			papc_angle = pa->normal_x*pc->normal_x + pa->normal_y*pc->normal_y + pa->normal_z*pc->normal_z;
			pbpc_angle = pb->normal_x*pc->normal_x + pb->normal_y*pc->normal_y + pb->normal_z*pc->normal_z;
		}
		if(counter >=  65){
			int ipa2;
			int ipb2;
			int ipc2;
			pcl::PointXYZRGBNormal * pa2;
			pcl::PointXYZRGBNormal * pb2;
			pcl::PointXYZRGBNormal * pc2;
			extractPoints(ipa2,ipb2,ipc2,pa2, pb2, pc2,valid_points,valid_counter);
			pa = pa2;
			pb = pb2;
			pc = pc2;
			ipa = ipa2;
			ipb = ipb2;
			ipc = ipc2;
		}
	}
	else{
		int ipa2;
		int ipb2;
		int ipc2;
		pcl::PointXYZRGBNormal * pa2;
		pcl::PointXYZRGBNormal * pb2;
		pcl::PointXYZRGBNormal * pc2;
		extractPoints(ipa2,ipb2,ipc2,pa2, pb2, pc2,valid_points,valid_counter);
		pa = pa2;
		pb = pb2;
		pc = pc2;
		ipa = ipa2;
		ipb = ipb2;
		ipc = ipc2;
	}
}
const bool debugg_extractPlanes = true;
void RGBDFrame::extractPlanes(pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr input_cloud){	
	ROS_INFO("extractPlanes 1");

	int max_planes 			= 10;
	int nr_random 			= 4000;
	
	float thickness_small	= 0.025f;
	float thickness_large	= thickness_small*2;
	
 
	
	int valid_counter		= 0;
	for(int i = 0; i < width*height; i++){
		if(input_cloud->points[i].z > 0){valid_counter++;}
	}
	//int valid_counter_backup	= 0;
	pcl::PointXYZRGBNormal ** valid_points 			= new pcl::PointXYZRGBNormal*[valid_counter];
	int* w_valid_points;
	int* h_valid_points;
	if(debugg_extractPlanes){
		w_valid_points 							= new int[valid_counter];
		h_valid_points 							= new int[valid_counter];
	}
	
	IplImage * rgb_img 			= cvCreateImage(cvSize(width, height), IPL_DEPTH_8U, 3);
	char * rgb_data 			= rgb_img->imageData;

	
	int counter = 0;
	int index = 0;
	for(int j = 0; j < height; j++){
		for(int i = 0; i < width; i++){
			if(input_cloud->points[index].z > 0 /*&& !std::isnan(input_cloud->points[index].normal_x)*/)
			{
				valid_points[counter] 			= &(input_cloud->points[index]);
				if(debugg_extractPlanes){
					w_valid_points[counter] = i;
					h_valid_points[counter]	= j;
				}
				counter++;
			}
			int ind = 3 * (j * width + i);
			rgb_data[ind + 0]=input_cloud->points[index].b; // B
			rgb_data[ind + 1]=input_cloud->points[index].g; // G
			rgb_data[ind + 2]=input_cloud->points[index].r; // R
			index++;
		}
	}
	
	float * steps 			= new float[32]; steps[0] = 200;
	for(int i = 1; i < 32; i++){steps[i] = steps[i-1]*2;}
	
	struct timeval start, end;
	gettimeofday(&start, NULL);


	for(int pl_number = 0; pl_number < max_planes; pl_number++)
	{
		float best_percentage = 0;
		int best = 0;
		vector<pcl::PointXYZRGBNormal *> * best_points = 0;
		Plane * best_plane = 0;

		for(int i = 0; i < nr_random; i++)//Ransac loop.
		{
			//printf("iter:%i\n",i);
			pcl::PointXYZRGBNormal * pa;
			pcl::PointXYZRGBNormal * pb;
			pcl::PointXYZRGBNormal * pc;
			
			int ipa;
			int ipb;
			int ipc;
			
			extractPoints(ipa, ipb, ipc, pa, pb, pc, valid_points, valid_counter);
			
			vector<pcl::PointXYZRGBNormal *> * plane_points = new vector<pcl::PointXYZRGBNormal *>();
			plane_points->push_back((pcl::PointXYZRGBNormal *)pa);
			plane_points->push_back((pcl::PointXYZRGBNormal *)pb);
			plane_points->push_back((pcl::PointXYZRGBNormal *)pc);
			
			Plane* current_plane = new Plane(plane_points);//Setup a new plane to test
			float pnx = current_plane->normal_x;
			float pny = current_plane->normal_y;
			float pnz = current_plane->normal_z;
			
			float angle_a = pa->normal_x*pnx+pa->normal_y*pny+pa->normal_z*pnz;
			float angle_b = pb->normal_x*pnx+pb->normal_y*pny+pb->normal_z*pnz;
			float angle_c = pc->normal_x*pnx+pc->normal_y*pny+pc->normal_z*pnz;

			int close_points = 0;
			int counter = 0;
			if((fabs(angle_a)+fabs(angle_b)+fabs(angle_c))>=3*(1-max_angle_diff))
			{
				for(int k = 0; k < 32 && counter < valid_counter;k++)
				{
					int mini = 0;
					if(valid_counter < steps[k]){mini = valid_counter;}
					else						{mini = steps[k];}
					float percentage_so_far = ((float)close_points)/((float)counter);
					if(k == 0 || (percentage_so_far > best_percentage*1.0f && percentage_so_far > 0.1))
					{ 
						for(;counter < mini;counter++)
						{
							pcl::PointXYZRGBNormal * point = valid_points[counter];
							float normmul = fabs(pnx*point->normal_x + pny*point->normal_y + pnz*point->normal_z);
							if(normmul > 1-max_angle_diff){
								float dist = fabs(current_plane->distance(point));
								if(dist < thickness_small * normmul){close_points++;}
							}
						}
					}
				}
			}
			if(close_points > best)
			{
				//ROS_INFO("Close_points: %i",close_points);
				best 				= close_points;
				best_percentage		= ((float)best)/((float)valid_counter);
				if(best_plane!=0)	{delete best_plane;}
				if(best_points!=0)	{delete best_points;}
				best_plane = current_plane;
				best_points = plane_points;
			}
			else{//delete current_plane;
				delete plane_points;
				delete current_plane;
			}
			
		}
		
		//if(best < 0.2*valid_counter){break;}
		
		char rcol = rand()%256;
		char gcol = rand()%256;
		char bcol = rand()%256;
		vector<pcl::PointXYZRGBNormal *> * best_plane_points = new vector<pcl::PointXYZRGBNormal *>();
		int q = 0;
		float pnx = best_plane->normal_x;
		float pny = best_plane->normal_y;
		float pnz = best_plane->normal_z;
		while(q < valid_counter)		//and go through all the points to check wich points fits
		{
			pcl::PointXYZRGBNormal * point = valid_points[q];
			//float normmul = fabs(pnx*point->normal_x + pny*point->normal_y + pnz*point->normal_z);
			float normmul = fabs(pnx*point->normal_x + pny*point->normal_y + pnz*point->normal_z);
			if(normmul > 1-max_angle_diff && fabs(best_plane->distance(point)) < thickness_large * normmul)	//if a point is close to the plane
			{
				best_plane_points->push_back(valid_points[q]);//Use that point for splitting the plane into sub-planes
				pcl::PointXYZRGBNormal * tmp = valid_points[valid_counter-1];
				valid_points[valid_counter-1] = valid_points[q];
				valid_points[q] = tmp;
				if(debugg_extractPlanes){
					int ind = 3 * (h_valid_points[q] * width + w_valid_points[q]);
					rgb_data[ind + 0]=bcol; // B
					rgb_data[ind + 1]=gcol; // G
					rgb_data[ind + 2]=rcol; // R
					
					int wtmp = w_valid_points[valid_counter-1];
					w_valid_points[valid_counter-1] = w_valid_points[q];
					w_valid_points[q] = wtmp;
					int htmp = h_valid_points[valid_counter-1];
					h_valid_points[valid_counter-1] = h_valid_points[q];
					h_valid_points[q] = htmp;
				}
				valid_counter--;
			}else{
				q++;
			}
		}
		ROS_INFO("best: %i",best);
		ROS_INFO("valid_counter: %i",valid_counter); 
		cvNamedWindow("InputCloud_rgb_planes", CV_WINDOW_AUTOSIZE );
		cvShowImage("InputCloud_rgb_planes", rgb_img);
		cvWaitKey(0);

		/*
		cvNamedWindow("rgb_img_disp", CV_WINDOW_AUTOSIZE );
		cvShowImage("rgb_img_disp", rgb_img_disp);
		ROS_INFO("\nDiff fast: %f\nbest: %i",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f,best);
		cvWaitKey(0);
		cvReleaseImage( &rgb_img_disp );
		

		std::vector< Plane *> * better_planes = last_plane->extract_subplanes(last_plane_points);
		int nr_better_planes = better_planes->size();
		for(int k = 0; k < nr_better_planes;k++)
		{
			planes.push_back(better_planes->at(k));
		}
		nr_valid_points		= 0;
		for(int q = 0; q < nr_points; q++)		
		{
			Point * p = points[q];
			for(int k = 0; k < nr_better_planes;k++)
			{
				Plane * plane_ptr = better_planes->at(k);
				if(fabs(plane_ptr->distance(p))<thickness_large)	//if a point is close to the plane
				{
					point_allocation[q]->push_back(plane_ptr);				// store the connection
				}
			}
			
			if(point_allocation[q]->empty())
			{
				valid_points[nr_valid_points] = p;
				nr_valid_points++;
			}
		}

		delete better_planes;
		*/
	}
	gettimeofday(&end, NULL);
	ROS_INFO("\Plane extraction execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	ROS_INFO("DONE\n");
	cvWaitKey(0);
	cvReleaseImage( &rgb_img );
}

void RGBDFrame::extractPlanes(Point *** pointMat, Point ** points,int nr_points, float ** point_x, float ** point_y, float ** point_z, IplImage * rgb_img, IplImage * depth_img){	
	ROS_INFO("extractPlanes 1");

	int max_planes 			= 10;
	int nr_random 			= 10000;
	
	float thickness_small	= 7.5;
	float thickness_large	= 30;

	Point** valid_points 	= new Point*[nr_points];
	int nr_valid_points 	= nr_points;	
	float * steps 			= new float[32]; steps[0] = 200;
	for(int i = 1; i < 32; i++){steps[i] = steps[i-1]*2;}
	vector<Plane *> ** point_allocation = new vector<Plane *>*[nr_points];
	
	for(int i = 0; i < nr_points; i++)
	{
		point_allocation[i] = new vector<Plane *>();
		valid_points[i]		= points[i];
		if(points[i] == 0){
			ROS_INFO("zero...");
		}
	}
	
	struct timeval start, end;
	gettimeofday(&start, NULL);

	ROS_INFO("extractPlanes 2");

	for(int pl_number = 0; pl_number < max_planes; pl_number++)
	{
		float best_percentage = 0;
		int best = 0;
		vector<Point *> * best_points = 0;
		Plane * best_plane = 0;
		ROS_INFO("pl_number %i",pl_number);
		for(int i = 0; i < nr_random; i++)//Ransac loop.
		{
			Point * p_a = valid_points[rand()%nr_valid_points];
			Point * p_b = valid_points[rand()%nr_valid_points];
			Point * p_c = valid_points[rand()%nr_valid_points];
			vector<Point *> * plane_points = new vector<Point *>();
			plane_points->push_back(p_a);
			plane_points->push_back(p_b);
			plane_points->push_back(p_c);

			Plane* current_plane = new Plane(plane_points);//Setup a new plane to test
	
			int close_points = 0;
			int counter = 0;
			for(int k = 0; k < 32 && counter < nr_valid_points;k++)
			{
				int mini = 0;
				if(nr_points < steps[k]){mini = nr_points;}
				else					{mini = steps[k];}
				float percentage_so_far = ((float)close_points)/((float)counter);
				if(k == 0 || (percentage_so_far > best_percentage*1.0f && percentage_so_far > 0.1))
				{ 
					for(;counter < mini;counter++)
					{
						float dist = fabs(current_plane->distance(valid_points[counter]));
						if(dist < thickness_small){close_points++;}
					}
				}
			}
			
			if(close_points > best)
			{
				//ROS_INFO("Close_points: %i",close_points);
				best 				= close_points;
				best_percentage		= ((float)best)/((float)nr_points);
				if(best_plane!=0)	{delete best_plane;}
				if(best_points!=0)	{delete best_points;}
				best_plane = current_plane;
				best_points = plane_points;
			}
			else{//delete current_plane;
				delete plane_points;
				delete current_plane;
			}
			
		}
		if(best < 0.1*nr_valid_points){break;}
		//gettimeofday(&end, NULL);
		//ROS_INFO("\nDiff fast: %f\nbest: %i",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f,best);
		IplImage* rgb_img_disp = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_UNCHANGED);

		Plane * last_plane 	= best_plane;	//Get the plane we just found
		vector<Point *> * last_plane_points = new vector<Point *>();
		//nr_valid_points		= 0;
		for(int q = 0; q < nr_points; q++)		//and go through all the points to check wich points fits
		{
			if(fabs(last_plane->distance(points[q]))<thickness_large)	//if a point is close to the plane
			{
				last_plane_points->push_back(points[q]);//Use that point for splitting the plane into sub-planes
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 0] = (unsigned char)255;
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 1] = 0;
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 2] = (unsigned char)255;
			}
		}
		
		cvNamedWindow("rgb_img_disp", CV_WINDOW_AUTOSIZE );
		cvShowImage("rgb_img_disp", rgb_img_disp);
		ROS_INFO("\nDiff fast: %f\nbest: %i",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f,best);
		cvWaitKey(0);
		cvReleaseImage( &rgb_img_disp );
		

		std::vector< Plane *> * better_planes = last_plane->extract_subplanes(last_plane_points);
		int nr_better_planes = better_planes->size();
		for(int k = 0; k < nr_better_planes;k++)
		{
			planes.push_back(better_planes->at(k));
		}
		nr_valid_points		= 0;
		for(int q = 0; q < nr_points; q++)		
		{
			Point * p = points[q];
			//if(!p_all_arr->empty() && p_all_arr->back() == last_plane){point_allocation[q]->pop_back();}
			for(int k = 0; k < nr_better_planes;k++)
			{
				Plane * plane_ptr = better_planes->at(k);
				if(fabs(plane_ptr->distance(p))<thickness_large)	//if a point is close to the plane
				{
					point_allocation[q]->push_back(plane_ptr);				// store the connection
				}
			}
			
			if(point_allocation[q]->empty())
			{
				valid_points[nr_valid_points] = p;
				nr_valid_points++;
			}
		}

		delete better_planes;
	}
	/*
	int nr_planes = planes.size();
	float ** prob_a_given_b = new float * [nr_planes];
	for(int i = 0; i < nr_planes;i++)
	{
		planes.at(i)->id = i;
		prob_a_given_b[i] = new float [nr_planes];
		for(int j = 0; j < nr_planes; j++)
		{
			prob_a_given_b[i][j] = 0;
		}
	}
	
	for(int q = 0; q < nr_points; q++)		//and go through all the points to check wich points fits
	{
		vector<Plane *>* plane_allocation_point = point_allocation[q];
		if(!plane_allocation_point->empty())
		{
			int nr_allocations = plane_allocation_point->size();
			for(int i = 0; i < nr_allocations; i++)
			{
				int id_i = plane_allocation_point->at(i)->id;
				prob_a_given_b[id_i][id_i]++;
				for(int j = 0; j < i; j++)
				{
					int id_j = plane_allocation_point->at(j)->id;
					prob_a_given_b[id_i][id_j]++;
					prob_a_given_b[id_j][id_i]++;
				}
			}
		}
	}
	
	for(int i = 0; i < nr_planes;i++)
	{
		float divisor = prob_a_given_b[i][i];
		for(int j = 0; j < nr_planes; j++)
		{
			ROS_INFO("%i,%i -> %f",i,j,prob_a_given_b[i][j]/divisor);
		}
	}
	for(int i = 0; i < nr_planes;i++)
	{
		IplImage* rgb_img_disp = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_UNCHANGED);
		for(int q = 0; q < nr_points; q++)		//and go through all the points to check wich points fits
		{
			if(fabs(planes.at(i)->distance(points[q]))<thickness_large)	//if a point is close to the plane
			{
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 0] = (unsigned char)255;
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 1] = 0;
				rgb_img_disp->imageData[3 * (points[q]->h * rgb_img_disp->width + points[q]->w) + 2] = (unsigned char)255;
			}
		}
		cvNamedWindow("rgb_img_disp", CV_WINDOW_AUTOSIZE );
		cvShowImage("rgb_img_disp", rgb_img_disp);
		cvWaitKey(0);
		cvReleaseImage( &rgb_img_disp );
	}
	*/
	gettimeofday(&end, NULL);
	ROS_INFO("\Plane extraction execution time: %fs",(end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f);
	ROS_INFO("DONE\n");
	//exit(0);
}

void RGBDFrame::showFrame()
{
    IplImage * rgb_img = cvLoadImage( rgb_path.c_str(), CV_LOAD_IMAGE_COLOR );
    IplImage * depth_img = cvLoadImage( depth_path.c_str(), CV_LOAD_IMAGE_COLOR );
	ROS_INFO("\nshow frame!\n%s\n%s",rgb_path.c_str(),depth_path.c_str());

	cvNamedWindow("rgb_pixel_image", CV_WINDOW_AUTOSIZE );
	cvShowImage("rgb_pixel_image", rgb_img);
	cvNamedWindow("depth_pixel_image", CV_WINDOW_AUTOSIZE );
	cvShowImage("depth_pixel_image", depth_img);
	
    //cvWaitKey(1000/10);
    cvReleaseImage( &rgb_img );
	cvReleaseImage( &depth_img );
}
